#ifndef RIKIBOT_DRIVER_HPP
#define RIKIBOT_DRIVER_HPP

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/BatteryState.h>
#include <geometry_msgs/Twist.h>
#include <serial/serial.h>

#define START_FRAME 0xABCD

#pragma pack(push, 1)  // 确保紧密打包，无填充字节
typedef struct {
    uint16_t start;
    int16_t rpmR;
    int16_t rpmL;
    int16_t batVoltage;
    int16_t boardTemp;
    int16_t curL_DC;
    int16_t curR_DC;
    uint16_t checksum;
} RikibotFeedback;

typedef struct {
    uint16_t start;
    int16_t mSpeedR;
    int16_t mSpeedL;
    uint16_t checksum;
} RikibotCommand;
#pragma pack(pop)  // 恢复默认打包

class RikibotDriver {
public:
    RikibotDriver(ros::NodeHandle &nh);
    void run();

private:
    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);
    void readSerial();
    void processFeedback(const RikibotFeedback &fb);
    uint16_t calculateChecksum(const RikibotFeedback &fb);
    uint16_t calculateCommandChecksum(const RikibotCommand &cmd);
    uint16_t ntohs(uint16_t value); // 处理字节序转换

    ros::NodeHandle nh_;
    ros::Publisher odom_pub_;
    ros::Publisher battery_pub_;
    ros::Subscriber cmd_vel_sub_;
    tf2_ros::TransformBroadcaster tf_broadcaster_;

    serial::Serial serial_;
    std::string port_;
    int baudrate_;
    std::string base_frame_id_;
    std::string odom_frame_id_;
    bool publish_tf_;
    bool swap_bytes_;  // 添加字节序交换标志

    double wheel_diameter_;
    double wheel_distance_;
    double wheel_circumference_;

    double x_;
    double y_;
    double theta_;
    ros::Time last_time_;
    
    // 串口缓冲区
    uint8_t serial_buffer_[256];
    size_t buffer_pos_;
};

#endif // RIKIBOT_DRIVER_HPP